
/*
* core zero, proc message.
*/

#include <stdint.h>
#include <stdio.h>

#include "rp2040.h"
#include "hardware/clocks.h"
#include "hardware/uart.h"

#include "pico.h"
#include "pico/multicore.h"

#include "umsg.h"
#include "dcchan.h"


static inline uint32_t  read_reg( uintptr_t addr )
{
    return (*((volatile uint32_t *) (addr)));
}

static inline void  write_reg( uintptr_t addr, uint32_t value )
{
  *(volatile uint32_t *)(addr) = value;
  return;
}


/*
输入:
u32 : addr
 u8 : cnt
*/
int core_zero_rreg( umsg_t * pmsg )
{
	uint32_t ofs;
	int cnt;
	uint8_t * ptr;
	int i;

	/**/
	ofs = umsg_get_u32( pmsg );
	cnt = umsg_get_u8( pmsg );
	if ( (cnt > 16) || (cnt < 1) ) {
		/* error */
		umsg_free( pmsg );
		return 0;
	}

	/**/
	umsg_reset_ofs( pmsg );
	for ( i=0; i<cnt; i++ ) {
		umsg_add_u32( pmsg, read_reg( ofs + (i * 4) ) );
	}

	/* 继承 type, 不需要修改. */
	umsg_send( pmsg );
	return 0;
}


int core_zero_wreg( umsg_t * pmsg )
{
	uintptr_t ofs;
	uint32_t vvv;
	uint8_t * ptr;
	int cnt;
	int i;

	if ( umsg_remains(pmsg) < 4 ) {
		umsg_add_i32( pmsg, 0xfffff );
		umsg_send( pmsg );
		return 1;
	}

	/* addr, v0, v1, ... */
	ofs = umsg_get_ptr( pmsg );
	cnt = umsg_remains( pmsg ) / 4;

	for ( i=0; i<cnt; i++ ) {
		vvv = umsg_get_u32( pmsg );
		write_reg( ofs + (i * 4), vvv );
	}

	/**/
	umsg_reset_ofs( pmsg );
	umsg_add_i32( pmsg, 0 );
	umsg_send( pmsg );
	return 0;
}


/*
 * 使用字节宽度, 访问总线.
 * 获得一块内存访问的内容.
 * 主要是对 SRAM 有意义.
 * input layout
 * {
 *     uint32_t  addr;
 *      uint8_t  cnt;
 * }
 *
 */
int core_zero_dump( umsg_t * pmsg )
{
	int cnt;
	uint8_t * ptr;
	volatile uint8_t * pmem;
	int i;

	/**/
	pmem = (volatile uint8_t *)umsg_get_ptr( pmsg );
	cnt = umsg_get_u8( pmsg );
	if ( cnt > 63 ) {
		umsg_add_i32( pmsg, 0xfffff );
		umsg_send( pmsg );
		return 0;
	}

	/**/
	umsg_reset_ofs( pmsg );
	for ( i=0; i<cnt; i++ ) {
		umsg_add_u8( pmsg, pmem[i] );
	}

	/**/
	umsg_send( pmsg );
	return 0;
}


int core_zero_clock( umsg_t * pmsg )
{
	int idx;
	int i;
	uint8_t * ptr;
	uint32_t  hzhz;

	/**/
	idx = umsg_get_u8( pmsg );
	hzhz = clock_get_hz( idx );

	/**/
	umsg_reset_ofs( pmsg );
	umsg_add_u32( pmsg, hzhz );

	/**/
	umsg_send( pmsg );
	return 0;
}


int core_zero_had_async( umsg_t * pmsg )
{
	uint8_t subcmd;

	/**/
	subcmd = umsg_get_u8( pmsg );
	
	switch ( subcmd ) {
	case 0x09:
		dmi_umsg_tclk_get( pmsg );
		break;

	case 0x10:
		dmi_umsg_tclk_set( pmsg );
		break;

	case 0x11:
		had_umsg_null_clock( pmsg );
		break;
	
	case 0x12:
		had_umsg_ir5_read( pmsg );
		break;

	case 0x13:
		had_umsg_ir5_write( pmsg );
		break;

	case 0x14:
		had_umsg_dr32_read( pmsg );
		break;

	case 0x15:
		had_umsg_dm_read( pmsg );
		break;

	case 0x16:
		had_umsg_dm_write( pmsg );
		break;
	
	case 0x18:
		dmi_umsg_sync( pmsg );
		break;
	
	case 0x19:
		dmi_umsg_leave_debug( pmsg );
		break;
		
	case 0x1A:
		dmi_umsg_write_mem( pmsg );
		break;

	case 0x1B:
		dmi_umsg_read_mem( pmsg );
		break;

	case 0x20:
		dmi_umsg_cpu_read( pmsg );
		break;

	case 0x21:
		dmi_umsg_cpu_write( pmsg );
		break;

	default:
		umsg_free( pmsg );
		break;
	}

	return 0;
}


int czo_common_cmd( umsg_t * pmsg )
{
	uint8_t subcmd;

	/**/
	subcmd = umsg_get_u8( pmsg );

	switch ( subcmd ) {

	case 0x0:
		/* echo */
		pmsg->offs = pmsg->mlen;
		umsg_send( pmsg );
		break;
		
	case 0x1:
		core_zero_rreg( pmsg );
		break;

	case 0x2:
		core_zero_wreg( pmsg );
		break;

	case 0x3:
		core_zero_dump( pmsg );
		break;

	default:
		umsg_free( pmsg );
		break;
	}

	return 0;
}


extern int had_debug_msg( umsg_t * pmsg );

void  core0_proc_msg( umsg_t * pmsg )
{
	switch ( pmsg->type) {

	case 0x0:
		czo_common_cmd( pmsg );
		break;

	case 0x8:
		core_zero_clock( pmsg );
		break;

	case 0x9:
		core_zero_had_async( pmsg );
		break;

	default:
		/* type = [0x00,0x3F] : forward to core-1 */
		pmsg->type = pmsg->type & 0x3F;
		multicore_fifo_push_blocking( (uint32_t)(uintptr_t)pmsg );
		break;
	}

	return;
}


